﻿#region Copyright Notice

//Copyright © 2007-2011, PARROT SA, all rights reserved. 

//DISCLAIMER 
//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
//software, even if advised of the possibility of such damage. 

//Author            : Wilke Jansoone
//Email             : wilke.jansoone@digitude.net
//Publishing date   : 28/11/2010 

//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
//are met:
//    - Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
//    - Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
//      specific prior written permission.

#endregion

#region Imports

using System;
using System.Timers;
using System.Threading;

using WiimoteLib;

using Wilke.Interactive.Drone.Contracts;

#endregion

namespace Wilke.Interactive.Drone.InputProviders
{
    /// <summary>
    /// This class captures input coming from a Wiimote controller. This class provides a variation on the standard WiimoteInputProvider in that it
    /// ignores the accelerometer input. This inputprovider is used during automated flights. These flights are controlled by a for example a workflow. In order
    /// to be able to respond to troublesome situations you need an inputprovider that you can hold and use to make slight corrections or land
    /// the drone.
    /// </summary>
    public class AssistInputProvider : IInputProvider
    {
        #region Private Fields

        private Wiimote wiimote = new Wiimote();
        private Random random = new Random();

        private Single rollThrottleValue;
        private Single pitchThrottleValue;
        private Single heightThrottleValue;
        private Single yawThrottleValue;

        #endregion

        #region Construction

        /// <summary>
        /// Initializes a new instance of the <see cref="AssistInputProvider"/> class.
        /// </summary>
        /// <param name="droneCommander">The drone commander.</param>
        public AssistInputProvider(IDroneCommander droneCommander)
        {
            this.DroneCommander = droneCommander;
        }

        #endregion

        #region Event Handlers

        private int throttler = 5000;

        /// <summary>
        /// Handles the WiimoteChanged event of the wm control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="WiimoteLib.WiimoteChangedEventArgs"/> instance containing the event data.</param>
        private void wm_WiimoteChanged(object sender, WiimoteChangedEventArgs e)
        {
            float roll = 0;
            float pitch = 0;
            float height = 0;
            float yaw = 0;

            try
            {
                if (throttler > 10)
                {
                    #region FlightParameters

                    if (e.WiimoteState.ButtonState.Up)
                    {
                        pitch = -pitchThrottleValue;
                    }
                    else if (e.WiimoteState.ButtonState.Down)
                    {
                        pitch = pitchThrottleValue;
                    }
                    else
                    {
                        pitch = 0;
                    }

                    if (e.WiimoteState.ButtonState.Left)
                    {
                        roll = -rollThrottleValue;
                    }
                    else if (e.WiimoteState.ButtonState.Right)
                    {
                        roll = rollThrottleValue;
                    }
                    else
                    {
                        roll = 0;
                    }

                    if (e.WiimoteState.ButtonState.Minus)
                    {
                        yaw = -yawThrottleValue;
                    }

                    if (e.WiimoteState.ButtonState.Plus)
                    {
                        yaw = yawThrottleValue;
                    }

                    height = 0;

                    DroneCommander.SetFlightParameters(roll, pitch, height, yaw);

                    #endregion

                    if (e.WiimoteState.ButtonState.A)
                    {
                        DroneCommander.StartEngines();
                    } 

                    if (e.WiimoteState.ButtonState.B)
                    {
                        DroneCommander.StopEngines();
                    }                  

                    throttler = 0;
                }

                throttler++;
            }
            catch
            {
                throw;
            }
        }

        #endregion

        #region IInputProvider Members

        /// <summary>
        /// Gets or sets the instance that implements the <see cref="IDroneCommander">IDroneCommander</see> interface.
        /// </summary>
        /// <value>
        /// The object that implements the <see cref="IDroneCommander">IDroneCommander</see> interface.
        /// </value>
        public IDroneCommander DroneCommander { get; set; }

        /// <summary>
        /// Initializes the specified input provider.
        /// </summary>
        /// <returns>
        /// Returns <c>true</c> if the provider was initialized correclty; otherwise, <c>false</c>.
        /// </returns>
        /// <remarks>The throttling values are implicitly set to 1, so the values registered by the inputprovider will be passed unchanged to the object that implements the <see cref="IDroneCommander">IDroneCommander</see> interface.</remarks>
        public bool Initialize()
        {
            return Initialize(1, 1, 1, 1);
        }

        /// <summary>
        /// Initializes the specified input provider.
        /// </summary>
        /// <param name="rollThrottle">The roll throttle value between 0 and 1. The actual throttle value registered by the inputprovider will be multiplied by this value.</param>
        /// <param name="pitchThrottle">The pitch throttle value between 0 and 1. The actual pitch value registered by the inputprovider will be multiplied by this value.</param>
        /// <param name="heightThrottle">The height throttle value between 0 and 1. The actual height value registered by the inputprovider will be multiplied by this value.</param>
        /// <param name="yawThrottle">The yaw throttle value between 0 and 1. The actual yaw value registered by the inputprovider will be multiplied by this value.</param>
        /// <returns>
        /// Returns <c>true</c> if the provider was initialized correclty; otherwise, <c>false</c>.
        /// </returns>
        /// <remarks>The throttling is applied in order to be able to control the ARDrone in smaller places. A throttle value of 1 means that there is no throttling, a value of 0 means that the resulting value will also be 0.</remarks>
        public bool Initialize(float rollThrottle, float pitchThrottle, float heightThrottle, float yawThrottle)
        {
            this.rollThrottleValue = rollThrottle;
            this.pitchThrottleValue = pitchThrottle;
            this.heightThrottleValue = heightThrottle;
            this.yawThrottleValue = yawThrottle;

            try
            {
                wiimote.Connect();
                wiimote.WiimoteChanged += wm_WiimoteChanged;
                wiimote.SetLEDs(false, false, true, false);
                return true;
            }
            catch
            {
                return false;
            }
        }

        #endregion

        #region IDisposable Members

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        public void Dispose()
        {
            if (wiimote != null)
            {
                wiimote.Disconnect();
                wiimote.Dispose();
                wiimote = null;
            }
        }

        #endregion      
    }
}
